﻿#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/boundary.h>
#include <pcl/filters/normal_space.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/keypoints/harris_3D.h>
#include <pcl/common/time.h> 
#include <time.h>
#include <pcl/console/time.h>
#include "ProcessData.h"

#define random(x) (rand()%x)
//#define    SIFT    1
using namespace std;

namespace pcl
{
	template<>
	struct SIFTKeypointFieldSelector<PointXYZI>
	{
		inline float
			operator () (const PointXYZI &p) const
		{
			return p.z;
		}
	};
}
typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> ColorHandlerT3;
typedef pcl::PointXYZI PointT;

typedef struct tagPOINT_4D
{
	double x;  //mm world coordinate x
	double y;  //mm world coordinate y
	double z;  //mm world coordinate z
	double i;  //mm world coordinate RGB
}POINT_WORLD;

pcl::PointCloud<PointT> cloud;
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_Hook(new pcl::PointCloud<PointT>);
const int coordinate = 0;
int ksearch_number = 50;
int max_iterations = 100;
int distance_threshold = 1.0;
int radius_search = 1.0;
int minneighbors = 200;
int ror_radius = 1.0;

int main()
{
	string filename = "./data/2020-05-18-11-25-32_Velodyne-HDL-32-Data";
	//savePCD(filename);
	//Processdata();
	pcl::PointCloud<PointT> cloud = readPCD(filename + ".pcd");
	pcl::PointCloud<PointT>::Ptr cloud_plane = PlaneDetection(cloud,ksearch_number, max_iterations, distance_threshold);
	pcl::PointCloud<PointT>::Ptr cloud_AfterPrePro = RorFilter(cloud_plane, minneighbors, ror_radius);
	pcl::PointCloud<PointT>::Ptr cloud_Hook = passthrough(cloud_AfterPrePro, coordinate);
	pcl::PointCloud<PointT>::Ptr cloud_shadow = AftershadowZ(cloud_Hook);
	pcl::PointCloud<PointT>::Ptr cloud_boundary = boundary_search(cloud_Hook, ksearch_number, radius_search);
	
#ifdef SIFT
	KeyPointDetection(cloud_Hook);
#else
	ConvexHull(cloud_Hook);
	Draw_Circle();
#endif

	//viewer(cloud,cloud.makeShared());
	//viewer(cloud, cloud_plane);
	//viewer(cloud, cloud_AfterPrePro);
	//viewer(cloud, cloud_Hook);
	//viewer(cloud, cloud_temp);
	//viewer(cloud, cloud_shadow);
	//viewer(cloud, cloud_boundary);
	
	return 0;
}